#include <iostream>
#include <fstream>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <vector>

#include "cv.h"
#include "highgui.h"

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/cloud_viewer.h>

#include <Eigen/Core> 
#include <Eigen/Geometry>

#include "graph/VertexCamera.h"
#include "graph/VertexPlane.h"
#include "graph/VertexPoint3D.h"
#include "graph/EdgeCameraPlane.h"



#include "FeatureDescriptor.h"
#include "SurfFeatureDescriptor64.h"
#include "SurfFeatureDescriptor128.h"
#include "OrbFeatureDescriptor.h"
#include "IntegerHistogramFeatureDescriptor.h"
#include "FloatHistogramFeatureDescriptor.h"

#include "Point.h"
#include "Plane.h"
#include "Line.h"
#include "KeyPoint.h"
#include "KeyPointSet.h"

#include "Association.h"
#include "KeyPointAssociation.h"
#include "PlaneAssociation.h"
#include "AssociationSet.h"

#include "FeatureExtractor.h"
#include "SurfExtractor.h"
#include "OrbExtractor.h"
#include "OwnFeatureExtractor.h"

#include "Map3D.cpp"
#include "RGBDFrame.cpp"
#include "Transformation.cpp"

#include "FeatureExtractor.cpp"
#include "SurfExtractor.cpp"
#include "OrbExtractor.cpp"
#include "FPFHExtractor.cpp"
#include "OwnFeatureExtractor.cpp"


#include "FrameMatcher.cpp"
#include "DummyMatcher.cpp"
#include "SurfRansacMatcherI.cpp"
#include "JCMergeMatcherI.cpp"
#include "JCSearchMatcherI.cpp"
#include "BasicIcpMatcher.cpp"
#include "BasicGIcpMatcher.cpp"
#include "NDTMatcher.cpp"
#include "KeyPointIcpMatcherI.cpp"
#include "AICK.cpp"
#include "PlanePostProcessMatcher.cpp"
#include "SACIAMatcher.cpp"
#include "RansacPCLCopyMatcher.cpp"
#include "RansacPCLCopySlowMatcher.cpp"

#include <pthread.h>
#include <sys/types.h>
#include <dirent.h>
#include <errno.h>

//#include "kmeans.cpp"

#include <iostream>
#include <fstream>

using namespace std;
string out_path = "output2/";

struct groundtruth {
	double timestamp;
	double tx;
	double ty;
	double tz;
	double qx;
	double qy;
	double qz;
	double qw;
};

struct timestamp_path {
	double timestamp;
	string path;
};

struct frame_data {
	timestamp_path * rgb;
	timestamp_path * depth;
	groundtruth * gt;
	RGBDFrame * frame;
};
vector< frame_data * > * getFrameData(string path,Map3D * map, int max){
	string path_gt		= path+"/groundtruth.txt";
	string path_rgb		= path+"/rgb.txt";
	string path_depth	= path+"/depth.txt";
	string path_asso	= path+"/asso_w_gt.txt";
	
	printf("starting setup\n");
	string line;	
	ifstream asso_file (path_asso.c_str());
	vector<frame_data *> frame_inputs;
	vector<FeatureDescriptor *> * centers = new vector<FeatureDescriptor *>();
	vector< frame_data * > * all_frames = new vector< frame_data * >();	
	int counter = 0;
	if (asso_file.is_open()){
		while ( asso_file.good() )
		{
			getline (asso_file,line);
			if(line[0] != '#'){
				
				int space1 = line.find(" ");
				if(space1 != -1 && counter < max){
					int space2  = line.find(" ",space1+1,1);
					int space3  = line.find(" ",space2+1,1);
					int space4  = line.find(" ",space3+1,1);
					
					int space5  = line.find(" ",space4+1,1);
					int space6  = line.find(" ",space5+1,1);
					int space7  = line.find(" ",space6+1,1);
					int space8  = line.find(" ",space7+1,1);
					int space9  = line.find(" ",space8+1,1);
					int space10 = line.find(" ",space9+1,1);
					int space11 = line.find(" ",space10+1,1);
					int space12 = line.find(" ",space11+1,1);
					
					timestamp_path * tp_depth 	= new timestamp_path();
					tp_depth->timestamp 		= atof(line.substr(0,space1).c_str());
					tp_depth->path				= path+"/"+line.substr(space1+1,space2-space1-1).c_str();
					
					timestamp_path * tp_rgb = new timestamp_path();
					tp_rgb->timestamp 		= atof(line.substr(space2+1,space3-space2-1).c_str());
					tp_rgb->path			= path+"/"+line.substr(space3+1,space4-space3-1).c_str();

					double ts			= atof(line.substr(space4+1,space5-space4-1).c_str());
					double tx			= atof(line.substr(space5+1,space6-space5-1).c_str());
					double ty			= atof(line.substr(space6+1,space7-space6-1).c_str());
					double tz			= atof(line.substr(space7+1,space8-space7-1).c_str());
					double qx			= atof(line.substr(space8+1,space9-space8-1).c_str());
					double qy			= atof(line.substr(space9+1,space10-space9-1).c_str());
					double qz			= atof(line.substr(space10+1,space11-space10-1).c_str());
					double qw			= atof(line.substr(space11+1,space12-space11-1).c_str());
					
					groundtruth * gt = new groundtruth();
					gt->timestamp 	= atof(line.substr(space4+1,space5-space4-1).c_str());
					gt->tx 			= atof(line.substr(space5+1,space6-space5-1).c_str());
					gt->ty 			= atof(line.substr(space6+1,space7-space6-1).c_str());
					gt->tz 			= atof(line.substr(space7+1,space8-space7-1).c_str());
					gt->qx 			= atof(line.substr(space8+1,space9-space8-1).c_str());
					gt->qy 			= atof(line.substr(space9+1,space10-space9-1).c_str());
					gt->qz 			= atof(line.substr(space10+1,space11-space10-1).c_str());
					gt->qw 			= atof(line.substr(space11+1,space12-space11-1).c_str());
					
					frame_data * fd = new frame_data();
					fd->rgb 		= tp_rgb;
					fd->depth 		= tp_depth;
					fd->gt 			= gt;
					
					
					Frame_input * fi =  new Frame_input();
					fi->depth_timestamp		= tp_depth->timestamp;
					fi->depth_path			= tp_depth->path;
					fi->rgb_timestamp		= tp_rgb->timestamp;
					fi->rgb_path 			= tp_rgb->path;
					fi->calibration			= map->extractor->calibration;
					fd->frame		= new RGBDFrame(fi,map);
					all_frames->push_back(fd);
				}
				counter++;
			}
		}
		asso_file.close();
	}else{cout << "Unable to open " << path_asso;}
	return all_frames;
}

vector<FeatureDescriptor * > * kmeans(vector<FeatureDescriptor *> input, int nr_restarts, int iterations, int nr_clusters)
{
	printf("doing kmeans with %i features\n",input.size());
	
	float best_sum = -1;
	vector<FeatureDescriptor * > * best_centers = new vector<FeatureDescriptor * >();
	for(int restart = 0; restart < nr_restarts; restart++)
	{
		printf("--------------------------------------------------------------------------------\n");
		
		vector<FeatureDescriptor * > * centers = new vector<FeatureDescriptor * >();
		for(int j = 0; j < nr_clusters; j++){centers->push_back(input.at(rand()%input.size())->clone());}
		float sum;
		
		for(int iter = 0; iter < iterations; iter++)
		{
			vector< vector<FeatureDescriptor * > * > * centers_data = new vector< vector <FeatureDescriptor * > * >();
			for(int j = 0; j < nr_clusters; j++){centers_data->push_back(new vector<FeatureDescriptor *>());}
			
			sum = 0;
			
			for(int i = 0; i < input.size(); i++)
			{
				FeatureDescriptor * current = input.at(i);
				float best = 99999;
				int best_id = -1;
				for(int j = 0; j < nr_clusters; j++)
				{
					float dist = current->distance(centers->at(j));
					
					if(best > dist){
						best = dist;
						best_id = j;
					}
				}
				sum+=best*best;
				centers_data->at(best_id)->push_back(current);
			}
		
			sum /= float(input.size());
			printf("errorsum: %f\n",sum);
	
			for(int j = 0; j < nr_clusters; j++){
				if(centers_data->at(j)->size() > 0){
					centers->at(j)->update(centers_data->at(j));
				}
			}

			for(int j = 0; j < nr_clusters; j++){delete centers_data->back(); centers_data->pop_back();}
			delete centers_data;
		}
		
		if(best_sum == -1 || sum < best_sum){
			best_centers = centers;
			best_sum = sum;
		}
	}
	return best_centers;
}

int main(int argc, char **argv)
{
	Calibration * calib0 = new Calibration();
	calib0->fx			= 525.0;
	calib0->fy			= 525.0;
	calib0->cx			= 319.5;
	calib0->cy			= 239.5;
	calib0->ds			= 1;
	calib0->scale		= 5000;
	
	Calibration * calib1 = new Calibration();
	calib1->fx			= 517.3;
	calib1->fy			= 516.5;
	calib1->cx			= 318.6;
	calib1->cy			= 255.3;
	calib1->ds			= 1.035;
	calib1->scale		= 5000;
	
	FeatureExtractor * orb = new OrbExtractor();
	FeatureExtractor * own = new OwnFeatureExtractor();
	SurfExtractor * surf = new SurfExtractor();
	orb->calibration = calib1;
	surf->calibration = calib1;
	own->calibration = calib1;
	
	segmenter = new RGBDSegmentation(calib1);
	
	Map3D * map = new Map3D();
	map->extractor = own;
	vector< frame_data * > * all_frames = getFrameData("/home/johane/test_data/rgbd_dataset_freiburg1_room",map,1);
	vector< FeatureDescriptor * > descriptors;
	for(int i = 0; i < all_frames->size(); i++){
		KeyPointSet * keypoints = all_frames->at(i)->frame->keypoints;
		for(int j = 0; j < keypoints->valid_key_points.size(); j++){descriptors.push_back(keypoints->valid_key_points.at(j)->descriptor);}
		for(int j = 0; j < keypoints->invalid_key_points.size(); j++){descriptors.push_back(keypoints->invalid_key_points.at(j)->descriptor);}
	}
	printf("input: %i\n",descriptors.size());
	//vector<FeatureDescriptor * > * bags = kmeans(descriptors, 3, 20, 25);
	/*
	for(int i = 0; i < bags->size(); i++){
		char buff[50];
		sprintf(buff,"bowTest1_%i.feature.orb",i);
		bags->at(i)->store(string(buff));
		bags->at(i)->print();
		new OrbFeatureDescriptor(string(buff));
	}
	*/
	/*
	for(int i = 0; i < bags->size(); i++){
		char buff[50];
		sprintf(buff,"bowTest2_%i.feature.FH",i);
		bags->at(i)->store(string(buff));
		bags->at(i)->print();
		new FloatHistogramFeatureDescriptor(string(buff));
	}
	*/
	return 0;
}
